#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"
#include "queue.h"
#include <math.h>
#include <string.h>
#include <stdio.h>

#include "driverLog.h"
#include "main_peripherals.h"
#include "driverSD_SPI_DMA_ISR_TASK.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "state.h"
#include "rtos_queues.h"
#include "commands.h"

char * name;
unsigned char byteLength;
unsigned char numValues;
unsigned char isSigned;
unsigned char intBits;
unsigned char floatBits;

const struct LogDefinition_t logDefinition[LOG_ID_MAX_VALUE] = {
    //                      NAME                            BYTES   COUNT   SIGNED, INTBITS, FLOATBITS
    [LOG_ID_TIME] =         {"Timestamp"                    ,4      ,1      ,0      ,32     ,0},
    [LOG_ID_TIME_CHAR] =    {"Timestamp (char)"             ,1      ,1      ,0      ,8      ,0},
    [LOG_ID_TIME_INC] =     {"Timestamp (inc)"              ,0      ,0      ,0      ,0      ,0},
    [LOG_ID_BATTERY] =      {"Battery"                      ,4      ,1      ,0      ,16     ,16},
    [LOG_ID_IMU_MODE] =     {"IMU Mode"                     ,1      ,1      ,0      ,8      ,0},
    [LOG_ID_ACC_RAW] =      {"Sens.Acc.Raw[x,y,z]"          ,6      ,3      ,1      ,16     ,0},
    [LOG_ID_ACC_FILTERED] = {"Sens.Acc.Filter[x,y,z]"       ,12     ,3      ,1      ,16     ,16},
    [LOG_ID_GYRO_RAW] =     {"Sens.Gyro.Raw[x,y,z]"         ,6      ,3      ,1      ,16     ,0},
    [LOG_ID_GYRO_FILTERED] ={"Sens.Gyro.Filter[x,y,z]"      ,12     ,3      ,1      ,16     ,16},
    [LOG_ID_MAGN_RAW] =     {"Sens.Magn.Raw [x,y,z]"        ,6      ,3      ,1      ,16     ,0},
    [LOG_ID_MAGN_FILTERED] ={"Sens.Magn.Filter[x,y,z]"      ,12     ,3      ,1      ,16     ,16},
    [LOG_ID_BARO_RAW] =     {"Sens.Baro.Raw"                ,4      ,1      ,1      ,32     ,0},
    [LOG_ID_BARO_FILTERED] ={"Sens.Baro.Filter"             ,4      ,1      ,1      ,16     ,16},
    [LOG_ID_IMU_HEADING] =  {"IMU.Heading[pitch,roll,yaw]"  ,12     ,3      ,1      ,16     ,16},
    [LOG_ID_RC_INPUT] =     {"RC.Input[0,1,2,3,4,5,6,7]"    ,16     ,8      ,1      ,16     ,0},
    [LOG_ID_MOTOR_1] =      {"Motor.1[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_2] =      {"Motor.2[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_3] =      {"Motor.3[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_4] =      {"Motor.4[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_5] =      {"Motor.5[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_6] =      {"Motor.6[setpoint,feedback]"   ,4      ,2      ,1      ,16     ,0},
    [LOG_ID_MOTOR_DEBUG] =  {"MotorDebug[set,fb1,fb2,fb3]"       ,8      ,4      ,1      ,16     ,0},
    [LOG_ID_MOTOR_BODE]   = {"MotorBode[error,pid,signal]"      ,6      ,3      ,1      ,16     ,0},
    [LOG_ID_POWER_CURRENT]   = {"Sense[Lipo,Current]"      ,8      ,2      ,1      ,16     ,16},
    [LOG_ID_PID_0] =        {"Pid.0[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_1] =        {"Pid.1[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_2] =        {"Pid.2[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_3] =        {"Pid.3[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_4] =        {"Pid.4[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_5] =        {"Pid.5[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_6] =        {"Pid.6[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_7] =        {"Pid.7[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_8] =        {"Pid.8[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_9] =        {"Pid.9[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_10] =        {"Pid.10[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_11] =        {"Pid.11[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_12] =        {"Pid.12[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_13] =        {"Pid.13[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_14] =        {"Pid.14[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
    [LOG_ID_PID_15] =        {"Pid.15[setpoint,feedback,output]",12   ,3      ,1      ,16     ,16},
};

BYTE			realtimeLogBuffer[2][BT_LOG_BUFFER_SIZE];
BYTE *          btLogBufferPos;
BYTE *          btLogBufferPosEnd;
BOOL			btLogBufferOverflow;
TickType_t      btLogCurrentTick;
TickType_t      btLogLastSent;

struct LogBuffer_t logBuffer[LOG_BUFFERS];

BYTE *      	logPointer = logBuffer[0].writeBuffer;
BYTE *          logPointerEnd = logBuffer[0].writeBuffer + LOG_STORAGE_SIZE;
BYTE			logCurrentBuffer = 0;
TickType_t      sdLogCurrentTick;

void logSendDefinitions()
{
    int i;

    if (!rnClaimTx(configTICK_RATE_HZ)) return;

    BYTE * packet = rnGetTxBuffer();

    for (i=0;i<sizeof(logDefinition) / sizeof(struct LogDefinition_t);i++)
    {
        if (logDefinition[i].name == NULL) continue;
        int pos = 0;
        packet[pos++] = CMD_LOG_SEND_DEFINITIONS;
        packet[pos++] = i;
        pos = configFillString(packet, pos, logDefinition[i].name);
        packet[pos++] = logDefinition[i].byteLength;
        packet[pos++] = logDefinition[i].numValues;
        packet[pos++] = logDefinition[i].isSigned;
        packet[pos++] = logDefinition[i].intBits;
        packet[pos++] = logDefinition[i].floatBits;
        rnTransmitAndConfirmBlock(packet, pos);
    }
    
    packet[0] = CMD_LOG_SEND_DEFINITIONS;
    rnTransmitAndConfirmBlock(packet, 1);

    rnReleaseTx();
}

void btLogDataCheckTimestamp(TickType_t time)
{
	if (time != btLogCurrentTick)
	{
        if (btLogCurrentTick == 0 || (time - btLogCurrentTick) > 255)
        {
            if (btLogBufferPos + 5 >= btLogBufferPosEnd)
            {
                btLogBufferOverflow = TRUE;
                return;
            }
    		btLogCurrentTick = time;
            *(btLogBufferPos++) = LOG_ID_TIME;
            memcpy(btLogBufferPos, &btLogCurrentTick, 4);
    		btLogBufferPos += 4;
        }
        else if (btLogCurrentTick + 1 == time)
        {
            if (btLogBufferPos + 1 >= btLogBufferPosEnd)
            {
                btLogBufferOverflow = TRUE;
                return;
            }
    		btLogCurrentTick = time;
            *(btLogBufferPos++) = LOG_ID_TIME_INC;
        }
        else
        {
            if (btLogBufferPos + 2 >= btLogBufferPosEnd)
            {
                btLogBufferOverflow = TRUE;
                return;
            }
            *(btLogBufferPos++) = LOG_ID_TIME_CHAR;
            *(btLogBufferPos++) = time - btLogCurrentTick;
    		btLogCurrentTick = time;
        }
	}
}

void sdLogDataCheckTimestamp(TickType_t time)
{
	if (time != sdLogCurrentTick)
	{
		sdLogCurrentTick = time;
		if (logPointer + 5 >= logPointerEnd)
		{
            logSwitchBuffer();
            return;
		}

		*(logPointer++) = 0x00;

		memcpy(logPointer, &sdLogCurrentTick, 4);
        logPointer += 4;
	}
}

void logSwitchBuffer()
{
    int i;
    logBuffer[logCurrentBuffer].state = LOG_STATE_FILLED;
    for (i=0;i<LOG_BUFFERS;i++)
    {
        if (logBuffer[i].state == LOG_STATE_EMPTY)
        {
            logBuffer[i].state = LOG_STATE_IN_USE;
            logCurrentBuffer = i;
            logPointer = logBuffer[logCurrentBuffer].writeBuffer;
            logPointerEnd = logPointer + LOG_STORAGE_SIZE;

            *(logPointer++) = 0x00;
            memcpy(logPointer, &sdLogCurrentTick, 4);
            logPointer+=4;
            return;
        }
    }

    // buffer overflow; keep writing current buffer
    while(1) {
        portNOP();
    }
    logBuffer[logCurrentBuffer].state = LOG_STATE_WRITING;
    logPointer = logBuffer[logCurrentBuffer].writeBuffer;
    logPointerEnd = logPointer + LOG_STORAGE_SIZE;
    *(logPointer++) = 0x00;
    memcpy(logPointer, &sdLogCurrentTick, 4);
    logPointer+=4;
}

void logInit()
{
	logCurrentBuffer = 0;
	logPointer = logBuffer[logCurrentBuffer].writeBuffer;

    int i;
	for (i=1;i<LOG_BUFFERS;i++)
	{
		logBuffer[i].state = LOG_STATE_EMPTY;
	}

	logBuffer[0].state = LOG_STATE_IN_USE;

    btLogBufferPos = realtimeLogBuffer[0];
    btLogBufferPosEnd = btLogBufferPos + BT_LOG_BUFFER_SIZE;
    *(btLogBufferPos++) = CMD_LOG_DATA_REALTIME;
}

int rtLogLength;
BYTE * rtLogBuffer;

void logTick()
{
	int i;

    if (STATE.sdLogging.loggingEnabled)
    {
        for (i=0;i<LOG_BUFFERS;i++)
        {
            if (logBuffer[i].state == LOG_STATE_FILLED)
            {
                // should write
                logBuffer[i].state = LOG_STATE_WRITING;

                extern QueueHandle_t queSD;
                struct SdCommand_t sdWrite;
                sdWrite.ref = (void*)&logBuffer[i];
                sdWrite.type = SDCMD_LOG;

                if (xQueueSendToBack(queSD, &sdWrite, 0) != pdPASS)
                {
                    logBuffer[i].state = LOG_STATE_EMPTY;
                    // TODO: proper error handling
                }
            }
        }
    }

    if (!STATE.btLogging.disableSending)
    {
        if (btLogBufferPos - btLogBufferPosEnd + BT_LOG_BUFFER_SIZE > BT_LOG_BUFFER_THRESHOLD ||
            (btLogBufferPos - btLogBufferPosEnd + BT_LOG_BUFFER_SIZE > 1 && btLogLastSent + configTICK_RATE_HZ / 10 < xTaskGetTickCount()))
        {
            if (rnClaimTx(0))
            {
                vTaskEnterCritical();
                {
                    rtLogLength = btLogBufferPos - btLogBufferPosEnd + BT_LOG_BUFFER_SIZE;
                    rtLogBuffer = btLogBufferPos - rtLogLength;
                    if (btLogBufferPos <= realtimeLogBuffer[1]) btLogBufferPos = realtimeLogBuffer[1];
                    else btLogBufferPos = realtimeLogBuffer[0];

                    btLogBufferPosEnd = btLogBufferPos + BT_LOG_BUFFER_SIZE;
                    *(btLogBufferPos++) = CMD_LOG_DATA_REALTIME;
                    btLogCurrentTick = 0;
                    btLogBufferOverflow	= 0;
                }
                vTaskExitCritical();

                btLogLastSent = xTaskGetTickCount();
                if (rtLogLength > 0)
                {
                    rnTransmitAndRelease(rtLogBuffer, rtLogLength);
                    rtLogLength = 0;
                }
            }
        }
    }
}


